function [gbest,gbestval,fitcount,fitness,gbestval_history]= FCO_func(fhd,Dimension,PopulationSize,MaxIter,lowerbound,upperbound,pos,bool_Plot,varargin)

rand('state',sum(100*clock));
mi = MaxIter;
ps = PopulationSize;
D = Dimension;
[pub, plb, vub, vlb] = SetBound(D,ps,lowerbound,upperbound);

%  Parameters of the Algorithm //////////////////////////////////////
cc=[2 2];  % For PSO
iwt=0.9-(1:mi).*(0.5./mi); % For PSO
G0 = 2.8; % For SGSA
epsilon = 1e-3; % For SGSA
g = 0.13; % For QGSA
NumSC = 5; % For Social Class
%  Parameters of the Algorithm //////////////////////////////////////

%  Settings of the Algorithm //////////////////////////////////////
% If ps = 50, NumSC = 4
% size_each_level = 12    12    12    14
% lb_level = 1    13    25    37
% ub_level = 12    24    36    50
size_each_level = repmat(floor(ps/NumSC),1,NumSC);
size_each_level(NumSC) = ps - (size_each_level(1)*(NumSC-1));
lb_level(1,1) = 1;
ub_level(1,1) = size_each_level(1);
for l = 2 : NumSC
    lb_level(l,1) = sum(size_each_level(1,1:l-1))+1;
    ub_level(l,1) = sum(size_each_level(1,1:l));
end
%  Settings of the Algorithm //////////////////////////////////////

vel = zeros(ps,D); % initialize the velocity of the particles
acc = zeros(ps,D); % initialize the acceleration of the particles

fitness = feval(fhd,pos', varargin{:});
fitcount = ps;
pbest = pos;
pbestval = fitness;
[gbest_fit, gbest_id] = min(pbestval);
gbest = pbest(gbest_id, :);
gbestrep = repmat(gbest, ps, 1);
cbest_fit = min(fitness); % Current best
cworst_fit = max(fitness); % Current worst
gbestval = gbest_fit;
gbestval_history = zeros(fix(mi/500),2);

if(bool_Plot)
    h = figure(8);
    haxes = plot( 0 , 0 );
    XArray = [];
    YArray = [];
    title('FCO');
end
dcc1 = ones(ps,D).*2;
dcc2 = ones(ps,D).*2;
diwt = ones(ps,D).*0.5;
leader = zeros(NumSC,D);
leader_id = zeros(NumSC,1);
leader_fit = zeros(NumSC,1);
leader_rep = zeros(ps,D);
temp_pos = zeros(ps,D);
temp_mass = zeros(ps,1);

if(cbest_fit == cworst_fit)
    mass = repmat((1/ps),ps,1);
else
    mass = (fitness'- cworst_fit) / (cbest_fit-cworst_fit);
    mass = mass / sum(mass);
end
sort_mass = [mass (1:ps)'];
sort_mass = sortrows(sort_mass,1);
std_mass = std(mass);

% Decide leaders for each country
dis = zeros(ps,1);
for n = 1: ps
    dis(n,1) = norm(pos(n,:)-gbest);
end
sort_dis = [dis (1:ps)'];
sort_dis = sortrows(sort_dis,1);
for l = 1 : NumSC
    leader_id(l,1) = sort_mass(ps-l+1,2);
%     leader_id(l,1) = sort_dis(lb_level(l,1),2);
    leader(l,:) = pos(leader_id(l,1),:);  
    leader_fit(l,1) = fitness(leader_id(l,1));  
    temp_pos(ub_level(l,1),:) = leader(l,:);
    temp_mass(ub_level(l,1),:) = mass(leader_id(l,1),:);
end
% Rearrange tho order of pos
count = 1;
for n = 1 : ps        
    if(sum(n==ub_level) == 0)            
        while(sum(count==leader_id))
            count = count + 1;
        end
        temp_pos(n,:) = pos(count,:);
        temp_mass(n,:) = mass(count,:);
        count = count + 1;
    end
end
pos = temp_pos;
mass = temp_mass;
leader_id = ub_level;
% Recode leader_fit
leader_fit_histroy = zeros(NumSC,mi);
leader_fit_histroy(:,1) = leader_fit;

for iter = 2 : mi             
    G = G0 * exp(1 - (iter / mi));
    leader_best = min(leader_fit);
    leader_worst = max(leader_fit);
    if(leader_best == leader_worst)
        leader_mass = repmat((1/NumSC),NumSC,1);
    else
        leader_mass = (leader_fit-leader_worst) / (leader_best-leader_worst);
        leader_mass = leader_mass / sum(leader_mass);
    end
    leader_mass_sort = [leader_mass (1:NumSC)'];
    leader_mass_sort = sortrows(leader_mass_sort, 1);
    leader_rank = [leader_mass_sort(:,2) (NumSC:-1:1)'];
    leader_rank = sortrows(leader_rank,1); % Smaller is better.
    % Leader movement
    if(iter ~= 2)
        for l = 1 : NumSC
%             if(l ~= leader_mass_sort(NumSC,2))
              if(leader_rank(l) ~= 1)  
                  force = zeros(1,D);
                  for k = 1 : NumSC
                      d = leader(l,:) - leader(k,:); % Repulsive force
                      force = force + (leader_mass(k)>leader_mass(l)) * -d.* rand(1,D) * leader_mass(k) / (norm(d) + epsilon);
                  end
%                   acc(leader_id(l),:) = G0 * force + 0.1 *rand(1,D) .* (perturb - leader(l,:));
                  acc(leader_id(l),:) = G * force;
              else
                  chosenone = fix(rand()*size_each_level(l))+lb_level(l);
                  S = rand(1,D);
                  u = rand(1,D);
                  S = (S>=0.5)-(S<0.5);
                  acc(leader_id(l),:) = g * S .* abs(leader(l,:) - pos(chosenone,:)) .* log(1./u);
            end
        end
    end
    leader_rep = zeros(ps,D);
    for l = 1 : NumSC % Announce leader for each country    
        lb = lb_level(l);
        ub = ub_level(l);
        N = size_each_level(l);    
        leader_rep(lb:ub,:) = repmat(leader(l,:),N,1);
        if(leader_rank(l) == 1)
%             acc(lb:ub,:) = cc(1).*rand(N,D).*(pbest(lb:ub,:)-pos(lb:ub,:)) + ...
%                            cc(2).*rand(N,D).*(leader_rep(lb:ub,:)-pos(lb:
%                            ub,:));
            group_mass = fitness(lb:ub);
            lbest = min(group_mass);
            lworst = max(group_mass);
            if(lbest == lbest)
                group_mass = repmat(1/N,N,1);
            else
                group_mass = (group_mass - lworst) / (lbest - lworst);
            end
            group_mass_sort = [group_mass (1:N)'];
            group_mass_sort = sortrows(group_mass_sort,1);
            numKbest = ceil(N * (1 - (iter /mi)));
            numKbest = (numKbest>1)*numKbest + (numKbest<=1)*1;
            for n = lb : ub
                acc(n,:) = zeros(1,D);
                for k = N : N-numKbest+1
                    force = zeros(1,D);
                    indexKbest = group_mass_sort(k, 2) + lb - 1;
                    force(1,:) = pos(indexKbest,:) - pos(n,:);
                    indexKbest = group_mass_sort(k, 2);
                    R = norm(force(1,:));
                    force(1,:) = force(1,:) * rand() * group_mass(indexKbest) / (R + epsilon);
                    acc(n,:) = acc(n,:) + force(1,:);
                end
                acc(n,:) = G * acc(n,:);
            end
        else
%             acc(lb:ub,:) = cc(1).*rand(N,D).*(pbest(lb:ub,:)-pos(lb:ub,:)) + ...
%                            cc(2).*rand(N,D).*(leader_rep(lb:ub,:)-pos(lb:ub,:));
            acc(lb:ub-1,:) = cc(2).*rand(N-1,D).*(leader_rep(lb:ub-1,:)-pos(lb:ub-1,:));
                             ((mod(iter,100)+1)/100)*cc(1).*rand(N-1,D).*(pbest(lb:ub-1,:)-pos(lb:ub-1,:));
        end
    end
    
    bound = 0.24 * (exp(-iter/mi))^6 + 0.01;
%     bound = 0.01 * (1-iter/mi);
%     vel = iwt(iter).*vel + acc;
%     vel = 0.5.*vel + acc;
    vel = rand(ps,D).*vel + acc;
        
    vel = ((vel>=vlb)&(vel<=vub)).*vel+(vel<vlb).*vlb+(vel>vub).*vub;
    pos = pos+vel;
    pos=((pos>=plb)&(pos<=pub)).*pos...
        +(pos<plb).*(plb+bound.*(pub-plb).*rand(ps,D))+(pos>pub).*(pub-bound.*(pub-plb).*rand(ps,D));
    
    fitness = feval(fhd,pos', varargin{:});
    fitcount = fitcount+ps;
    tmp = (pbestval < fitness);
    temp=repmat(tmp',1,D);
    pbest=temp.*pbest+(1-temp).*pos;
    pbestval=tmp.*pbestval+(1-tmp).*fitness; % update the pbest    
    [gbest_fit, gbest_id] = min(pbestval);
    gbest = pbest(gbest_id, :);
    gbestrep = repmat(gbest, ps, 1);
    cbest_fit = min(fitness); % Current best
    cworst_fit = max(fitness); % Current worst
    gbestval = gbest_fit;
        
    for l = 1 : NumSC
        if(mod(iter,1) == 0) % Choose new leader
            [leader_fit(l,1), leader_id(l,1)] = min(fitness(lb_level(l):ub_level(l)));
            leader_id(l,1) = leader_id(l,1)+lb_level(l)-1;
            leader(l,:) = pos(leader_id(l,1), :);
        else
            leader_fit(l,1) = fitness(leader_id(l,1));
            leader(l,:) = pos(leader_id(l,1), :);
        end
    end
%     if(mod(iter,100) == 0)
%         leader_id
%     end
    leader_fit_histroy(:,iter) = leader_fit;    
        
    if(bool_Plot & mod(iter,100)== 0)
        XArray = [ XArray iter]; 
        YArray = [ YArray gbestval];
        set( haxes , 'XData' , XArray , 'YData' , YArray );
        drawnow
    end
    
    if(mod(iter,500) == 0)
        gbestval_history(fix(iter/500),1) = gbestval;
        gbestval_history(fix(iter/500),2) = iter;
    end
end

% h = subplot(4,2,2);
% subplot(2,2,1);
% plot(leader_fit_histroy(1,:));
% dis1 = norm(leader(1,:)-leader(2,:));
% dis2 = norm(leader(1,:)-leader(3,:));
% dis3 = norm(leader(1,:)-leader(4,:));
% dis = [num2str(dis1) ', '  num2str(dis2) ', '  num2str(dis3)];
% title(['Leader 1: ' dis]);
% subplot(2,2,2);
% plot(leader_fit_histroy(2,:));
% dis1 = norm(leader(2,:)-leader(1,:));
% dis2 = norm(leader(2,:)-leader(3,:));
% dis3 = norm(leader(2,:)-leader(4,:));
% dis = [num2str(dis1) ', ' num2str(dis2) ', ' num2str(dis3)];
% title(['Leader 2: ' dis]);
% subplot(2,2,3);
% plot(leader_fit_histroy(3,:));
% dis1 = norm(leader(3,:)-leader(1,:));
% dis2 = norm(leader(3,:)-leader(2,:));
% dis3 = norm(leader(3,:)-leader(4,:));
% dis = [num2str(dis1) ', ' num2str(dis2) ', ' num2str(dis3)];
% title(['Leader 3: ' dis]);
% subplot(2,2,4);
% plot(leader_fit_histroy(4,:));
% dis1 = norm(leader(4,:)-leader(1,:));
% dis2 = norm(leader(4,:)-leader(2,:));
% dis3 = norm(leader(4,:)-leader(3,:));
% dis = [num2str(dis1) ', ' num2str(dis2) ', ' num2str(dis3)];
% title(['Leader 4: ' dis]);
% drawnow

end


